﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using Drifting.Foundation;

namespace Drifting.MotionPlanning.Systems
{
    public class Drift6D : BaseSystem
    {

        public double c0;
        public double c1;
        public double c2;
        public double c3;
        public double width;
        public double width_wheel;
        public double width_mirror;
        public double height;

        double mu_v2f;
        double f1_max;
        double f2_max;
        double M;
        double J;

        public Drift6D()
            : base("../../../MotionPlanning/Resources/Drift6D.kdl")
        {
            c0 = 0.805;         //m
            c1 = 1.3805;        //m
            c2 = 1.3805;        //m
            c3 = 1.049;         //m
            height = 1.424;     //m
            
            width_wheel = 1.540; //m
            width_mirror = 1.976;//m
            width = 1.821;      //m

            M = 1580;           //kg    1580
            J = 2740;           //kg m2   2740
           
            double vt = 31.2928; //m/s
            double dist = 44.6344; //m
            double dec = vt * vt / 2 / dist;
            double t_brake = vt / dec;
            double v_slide = vt / t_brake;
            double f_brake = dec * M; // N
            double f_brake_front = f_brake / 5 * 3;
            mu_v2f = f_brake_front / v_slide;

            double skidPad = 0.98; //g
            f1_max = M * skidPad / 2; //N
            f2_max = M * skidPad / 2; //N

            t_delta = 0.1;
        }

        override public void Qd(double[] q, double[] ctrl, ref double[] qd)
        {
            double x = q[0];
            double y = q[1];
            double tta = q[2];
            double s = q[3];
            double v = q[4];
            double w = q[5];
            double phi = ctrl[0];


            double fwB = ctrl[1];   //front wheel brake 0-1
            double fwA = ctrl[2];   //front wheel forward speed, when fwB == 0
            double rwB = ctrl[3];   //rear wheel brake 0-1
            double rwA = ctrl[4];   //rear wheel forward speed, when rwB == 0

            double cosT = Math.Cos(tta);
            double sinT = Math.Sin(tta);
            double cosFi = Math.Cos(phi);
            double sinFi = Math.Sin(phi);

            //Front wheel
            double vs_fframe = -s + 0 + 0;
            double vv_fframe = 0 + -v - w * c1;

            double vx_fwheel = cosFi * vs_fframe + sinFi * vv_fframe;
            double vy_fwheel = -sinFi * vs_fframe + cosFi * vv_fframe;
            if (fwB < 0) vx_fwheel = fwA;    //no brake
            else vx_fwheel *= fwB;   //with brake

            vs_fframe = cosFi * vx_fwheel - sinFi * vy_fwheel;
            vv_fframe = sinFi * vx_fwheel + cosFi * vy_fwheel;

            //Rear wheel
            double vs_rframe = -s + 0 + 0;
            double vv_rframe = 0 + -v + w * c2;
            if (rwB < 0) vs_rframe = rwA;     //no brake
            else vs_rframe *= rwB;     //with brake

            double f_front = Math.Sqrt(vs_fframe * vs_fframe + vv_fframe * vv_fframe) * mu_v2f;
            double f_rear = Math.Sqrt(vs_rframe * vs_rframe + vv_rframe * vv_rframe) * mu_v2f;

            double fs_front = vs_fframe * mu_v2f;
            double fv_front = vv_fframe * mu_v2f;
            double fs_rear = vs_rframe * mu_v2f;
            double fv_rear = vv_rframe * mu_v2f;

            if (f_front > f1_max)
            {
                fs_front = fs_front / f_front * f1_max;
                fv_front = fv_front / f_front * f1_max;
            }

            if (f_rear > f2_max)
            {
                fs_rear = fs_rear / f_rear * f2_max;
                fv_rear = fv_rear / f_rear * f2_max;
            }

            double ds = (fs_front + fs_rear) / M;
            double dv = (fv_front + fv_rear) / M;
            double dw = (c1 * fv_front - c2 * fv_rear) / J;
            double dx = s * cosT - v * sinT;
            double dy = s * sinT + v * cosT;
            double dtta = w;

            qd[0] = dx;
            qd[1] = dy;
            qd[2] = dtta;
            qd[3] = ds;
            qd[4] = dv;
            qd[5] = dw;
        }

        override public void Integration(MemoryPad pad, double[] q, double[] ctrl, double t_delta, ref double[] q_next)
        {
            Qd(q, ctrl, ref pad.Qd_out);
            MathAssistant.vector_kTimes(pad.Qd_out, t_delta, ref pad.k1);

            MathAssistant.vector_vlinear(pad.k1, 0.5, q, ref pad.Qd_in);
            Qd(pad.Qd_in, ctrl, ref pad.Qd_out);
            MathAssistant.vector_kTimes(pad.Qd_out, t_delta, ref pad.k2);

            MathAssistant.vector_vlinear(pad.k2, 0.5, q, ref pad.Qd_in);
            Qd(pad.Qd_in, ctrl, ref pad.Qd_out);
            MathAssistant.vector_kTimes(pad.Qd_out, t_delta, ref pad.k3);

            MathAssistant.vector_vlinear(pad.k3, 1, q, ref pad.Qd_in);
            Qd(pad.Qd_in, ctrl, ref pad.Qd_out);
            MathAssistant.vector_kTimes(pad.Qd_out, t_delta, ref pad.k4);

            MathAssistant.vector_vlinear(pad.k1, 1.0 / 6.0, q, ref pad.Qd_out);
            MathAssistant.vector_vlinear(pad.k2, 1.0 / 3.0, pad.Qd_out, ref pad.Qd_in);
            MathAssistant.vector_vlinear(pad.k3, 1.0 / 3.0, pad.Qd_in, ref pad.Qd_out);
            MathAssistant.vector_vlinear(pad.k4, 1.0 / 6.0, pad.Qd_out, ref q_next);

            while (q_next[2] > Math.PI)
                q_next[2] -= MathAssistant.PI_double;
            while (q_next[2] < -Math.PI)
                q_next[2] += MathAssistant.PI_double;
        }

    }
}
